#include "pid_controller.h"

#if (PID_USE_DEBUG == pdON)
#include "stdio_uc.h"
#endif

// SINTONIA PID

// Cada aplicação requer um ajuste nos parâmetros de controle PID para atingir um
// 		resultado satisfatósio, isto se chama sintonia PID.
// Métodos existentes para se fazer a sintonia:
//		Ziegler Nichols

// A sintonia PID é encontrar valores adequados para kP, kI e kD,
//	estes valores são encontrados na base de tentativa e erro.
//	Porém a técnicas para facilitar esse processo
//  1 - Ajustar kP:
//		Sempre começar com o valor kP bem baixo e o kI e kD = 0.0
//		Ajustar até chegar o melhor valor possivel.
//		Esse valor influencia diretamente na potencia desejada,
//		checar se a potencia maxima consegue o sistema chegar no ponto,
//		se sim e se o sistema consegue se manter é hora de ajusta o kI
//	2 - Ajustar kI:
//		Começar com o valor bem baixo


static volatile uint timeSamples;
static double newSample;
static uint timeSamplesLoad;
static double kP, kI, kD;
static double setPoint;
static double U, E, I;
static double deltaTime;
static uint run = 0;

// ---------------------------------------------------------------------------------------
// Descrição: Inicializa o controlador via PID
// Parâmetros:
//		ts: Tempo de amostragem em ms, em quanto em quanto tempo os cálculos serão feitos.
// 			Não adianta quere atuar rápido se o sistema muda devagar,
//				e se o mesmo mudar rápido também não adianta atuar devagar.
//			O tempo é diretamente proporcional ao tempo de resposta do sistema.
//			Para um sistema que tenha resposta rápida esse tempo deve ser bem curto,
//			como por exemplo controlar velocidade do motor ou temperatura do ar em um tubo.
//			Já em um sistema de resposta bem lento o tempo pode ser alto, como aquecer litros
//				de um determinado liquido
//		kP, kI e kD: São parâmetros de controle PID. Se refere a sintonia do PID.
//			Ver técnicas para encontrar estes valores
// ---------------------------------------------------------------------------------------
void pid_Init(uint ts, double _kP, double _kI, double _kD) {
	timeSamplesLoad = ts;
	kP = _kP;
	kI = _kI;
	kD = _kD;
	pid_Reset();
}

// ---------------------------------------------------------------------------------------
// Ajusta o setpoint desejado
// ---------------------------------------------------------------------------------------
void pid_SetPoint(double sp) {
	setPoint = sp;
}

// ---------------------------------------------------------------------------------------
// Sinaliza para iniciar ou parar os cáclulos PID. No momento de iniciar os valores
// dos cáclulos serão reiniciados
// ---------------------------------------------------------------------------------------
void pid_Run(uint _run) {
	if ( (run==0) && (_run==1)) pid_Reset(); // se é para ligar vamos reiniciar os valores
	run = _run;
}

// ---------------------------------------------------------------------------------------
// Alimenta a fórmula PID com uma nova amostragem e retorna com o valor PID cálculado
// Se o controlador PID estiver desligado o valor retornado será 0, senão será o valor PID
// ---------------------------------------------------------------------------------------
double pid_Values(double _newSample) {
	newSample = _newSample;
	if (run) return U; else return 0;
}

// ---------------------------------------------------------------------------------------
// Procedimento para reinicar o cáclulo PID
// ---------------------------------------------------------------------------------------
void pid_Reset(void) {
	timeSamples = 0;
	U=0;
	E=0;
	I=0;
}

// ---------------------------------------------------------------------------------------
// Procedimento para o cáclulo PID. Esta função deve ser chamada a cada 1ms
// ---------------------------------------------------------------------------------------
void pid_Tick(void) {
	static double e, D;

	if (!run) return;
	if (timeSamples--) return;
	timeSamples = timeSamplesLoad;

	deltaTime = timeSamplesLoad/1000.0;

 	e = E;						// Guardar o erro anteriror
 	E = setPoint - newSample; 	// quanto está errado
 	I = I + E*deltaTime;		// Calcular a integral
 	D = (E-e)/deltaTime;		// Calcular a derivada
 	U = kP*E + kI*I + kD*D;		// Calcular o PID

	//onde
	// U = nova saida
	// E = erro atual
	// e = erro anterior
	// I = Integral
	// D = Derivada
	// kP = constante proporcional
	// kI = constante integral
	// kD = constante derivativa

	#if (PID_USE_DEBUG == pdON)
	plognp("E:%f U:%f I:%f D:%f"CMD_TERMINATOR, E, U, kI*I, kD*D);
	#endif
}
